/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_state.h"
#include "rtthread.h"
#include "drv_led.h"
#include "drv_pin.h"
#include "drv_adc.h"
#include "drv_serial.h"

#include "zino.h"

#define DBG_TAG "fcState"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

uint32_t fc_states = 0;
uint32_t fc_states_pre = 0;
uint8_t motor_led_state[4];
static bool showing_bat = 0;
const uint8_t red_color[3] = {255, 0, 0};       // 红灯： 快闪：校准中， 慢闪：低电量 呼吸：充电中
const uint8_t greenColor[3] = {0, 255, 0};      // 绿灯： 快闪：解锁中， 慢闪：编程中 呼吸：待机    点闪：飞行中
const uint8_t blue_color[3] = {0, 0, 255};      // 蓝灯： 快闪：天线坏， 慢闪：对频中 呼吸：-----
const uint8_t yellow_color[3] = {200, 50, 0};   // 黄灯： 快闪：陀螺坏， 慢闪：无信号
const uint8_t purple_color[3] = {255, 0, 255};  // 紫灯： 快闪：气压坏， 慢闪：
const uint8_t cyan_color[3] = {0, 255, 255};    // 青灯： 快闪：激光坏， 慢闪：
const uint8_t white_color[3] = {255, 255, 255}; // 白灯： 快闪：光流坏， 慢闪：出厂测试中
struct rgbColorState
{
    const uint8_t *color;
    uint8_t mode;
};
const char fcstates_str[FC_ERROR_COUNT][32] = {
    "FC_ACC_GYRO_HARDFAULT",
    "FC_BARO_HARDFAULT",
    "FC_RC_HARDFAULT",
    "FC_OPTICALFLOW_HARDFAULT",
    "FC_COMPASS_HARDFAULT",
    "FC_IMU_CALIBRATING",
    "FC_RC_PAIRING",
    "FC_RC_LOST",
    "FC_COMPASS_CALIBRATING",
    "FC_OK_TO_ARM",
    "FC_EMERGENCY_STOP",
    "FC_ARMED",
    "FC_ARMING_ALERK",
    "FC_MODE_STANDARD_INDOOR",
    "FC_MODE_STANDARD_OUTDOOR",
    "FC_LOW_POWER",
    "FC_LASER_HARDFAULT",
    "FC_FACTORY_TEST",
    "FC_MANUAL_MODE",
    "FC_FLIPING_MODE",
    "FC_LANDING",
    "FC_LANDED",
    "FC_HEADLESS_MODE",
    "FC_PROGRAM_MODE",
    "FC_TAKINGOFF",
    "FC_AUTO_TAKINGOFF",
    "FC_AIRBORN",
    "FC_PRORAM_READY",
};
struct rgbColorState rgbColorState[FC_ERROR_COUNT] =
    {
        [FC_ACC_GYRO_HARDFAULT] = {yellow_color, RGB_STATE_MODE_FAST},
        [FC_BARO_HARDFAULT] = {purple_color, RGB_STATE_MODE_FAST},
        [FC_RC_HARDFAULT] = {blue_color, RGB_STATE_MODE_FAST},
        [FC_OPTICALFLOW_HARDFAULT] = {white_color, RGB_STATE_MODE_FAST},
        [FC_COMPASS_HARDFAULT] = {0, 0},
        [FC_IMU_CALIBRATING] = {red_color, RGB_STATE_MODE_FAST},
        [FC_RC_PAIRING] = {blue_color, RGB_STATE_MODE_SLOW},
        [FC_RC_LOST] = {yellow_color, RGB_STATE_MODE_SLOW},
        [FC_COMPASS_CALIBRATING] = {0, 0},
        [FC_OK_TO_ARM] = {greenColor, RGB_STATE_MODE_FAST},
        [FC_EMERGENCY_STOP] = {greenColor, RGB_STATE_MODE_FAST},
        [FC_ARMED] = {greenColor, RGB_STATE_MODE_DOT},
        [FC_ARMING_ALERK] = {greenColor, RGB_STATE_MODE_FAST},
        [FC_LOW_POWER] = {red_color, RGB_STATE_MODE_SLOW},
        [FC_LASER_HARDFAULT] = {cyan_color, RGB_STATE_MODE_FAST},
        [FC_FACTORY_TEST] = {white_color, RGB_STATE_MODE_DOT},
        [FC_MANUAL_MODE] = {0, 0},
        [FC_FLIPING_MODE] = {red_color, RGB_STATE_MODE_FAST},
        [FC_LANDING] = {yellow_color, RGB_STATE_MODE_ON},
        [FC_LANDED] = {greenColor, RGB_STATE_MODE_FAST},
        [FC_HEADLESS_MODE] = {0, 0},
        [FC_PROGRAM_MODE] = {0, 0},
        [FC_TAKINGOFF] = {0, 0},
        [FC_AUTO_TAKINGOFF] = {yellow_color, RGB_STATE_MODE_DOT},
        [FC_AIRBORN] = {greenColor, RGB_STATE_MODE_ON},
        [FC_PRORAM_READY] = {0, 0},
};

void motor_led_state_set(uint8_t index, rgb_state_t state)
{
    motor_led_state[index % 4] = state;
}
void show_battery_led(void)
{
    showing_bat = 1;
}
void fc_state_show(void)
{
    static uint32_t stateCnt = 0, showCnt = 0;
    uint8_t r = 0, g = 0, b = 0;
    if (fc_states != fc_states_pre)
    {
        fc_states_pre = fc_states;
        // LOG_D("FC State: %08X", fc_states);
    }
    showCnt++;
    if (showCnt > 32)
    {
        showCnt = 0;
        // // NOTE 一次只亮一种灯，只亮优先级最高的
        // for (stateCnt = 0; stateCnt < FC_ERROR_COUNT; stateCnt++)
        // {
        //     if (FC_STATE_CHECK(stateCnt))
        //     {
        //         LOG_D("ERROR %2d: %s\n", (int)stateCnt,fcstates_str[stateCnt]);
        //         break;
        //     }
        // }
        // for (uint8_t i = 0; i < FC_ERROR_COUNT; i++)
        // {
        //     if (FC_STATE_CHECK(i))
        //     {
        //         LOG_D("ERROR %2d: %s\n", (int)i, fcstates_str[i]);
        //     }
        // }
        if (fc_states)
        {
            while (1)
            {
                stateCnt++;
                if (stateCnt >= FC_ERROR_COUNT)
                {
                    stateCnt = 0;
                }
                if (fc_states == 0 || FC_STATE_CHECK(stateCnt))
                    break;
            }
            LOG_D("ERROR 0x%08X - %2d: %s\n", fc_states, (int)stateCnt, fcstates_str[stateCnt]);
            uart4_printf("ERROR 0x%08X - %2d: %s\n", fc_states, (int)stateCnt, fcstates_str[stateCnt]);
        }
    }

    if (fc_states)
    {
        if (rgbColorState[stateCnt].color != 0)
        {
            r = blinkValues[rgbColorState[stateCnt].mode] * rgbColorState[stateCnt].color[0] / 255;
            g = blinkValues[rgbColorState[stateCnt].mode] * rgbColorState[stateCnt].color[1] / 255;
            b = blinkValues[rgbColorState[stateCnt].mode] * rgbColorState[stateCnt].color[2] / 255;
        }
    }
    else
    {
        r = blinkValues[RGB_STATE_MODE_BREATH] * greenColor[0] / 255;
        g = blinkValues[RGB_STATE_MODE_BREATH] * greenColor[1] / 255;
        b = blinkValues[RGB_STATE_MODE_BREATH] * greenColor[2] / 255;
    }

    // LOG_D("d:%3d r: %3d, g: %3d, b: %3d \n",blinkValues[RGB_STATE_MODE_BREATH], r,g,b);
    rgb_set_color(0, r, g, b);
    rgb_set_color(1, r, g, b);
    rgb_show();

    show_battery_level_by_motor_led();

    led_set_brightness1(blinkValues[motor_led_state[0] % RGB_STATE_MODE_CNT]);
    led_set_brightness2(blinkValues[motor_led_state[1] % RGB_STATE_MODE_CNT]);
    led_set_brightness3(blinkValues[motor_led_state[2] % RGB_STATE_MODE_CNT]);
    led_set_brightness4(blinkValues[motor_led_state[3] % RGB_STATE_MODE_CNT]);
}

void show_battery_level_by_motor_led(void)
{
    if (!showing_bat)
        return;
    static uint8_t showCnt = 0;
    static uint8_t showStep = 0;
    // const uint16_t level[4]={3000,3600,3800,4000,4200};
    uint8_t bat_level = 1;
    if (get_vbat_voltage_mv() > 4000)
    {
        bat_level = 4;
    }
    else if (get_vbat_voltage_mv() > 3800)
    {
        bat_level = 3;
    }
    else if (get_vbat_voltage_mv() > 3600)
    {
        bat_level = 2;
    }
    else if (get_vbat_voltage_mv() > 3000)
    {
        bat_level = 1;
    }
    else
    {
        bat_level = 0;
    }

    if (bat_level == 0)
    {
        motor_led_state_set(0, RGB_STATE_MODE_DOT);
        motor_led_state_set(1, RGB_STATE_MODE_DOT);
        motor_led_state_set(2, RGB_STATE_MODE_DOT);
        motor_led_state_set(3, RGB_STATE_MODE_DOT);

        if (showCnt > 64)
        {
            showing_bat = 0;
        }
    }
    else
    {
        if (showStep == 0)
        {
            motor_led_state_set(0, RGB_STATE_MODE_OFF);
            motor_led_state_set(1, RGB_STATE_MODE_OFF);
            motor_led_state_set(2, RGB_STATE_MODE_OFF);
            motor_led_state_set(3, RGB_STATE_MODE_OFF);
            showStep = 1;
        }
        else
        {
            motor_led_state_set(showStep - 1, RGB_STATE_MODE_BREATH);
            // rt_kprintf("showStep:%d/%d\n", showStep, bat_level);
            showCnt++;
            if (showCnt > 12)
            {
                showStep++;
                showCnt = 0;
            }
            if (showStep > bat_level)
            {
                showing_bat = 0;
                showStep = 0;
                showCnt = 0;
            }
        }
    }
}
